/*
 * WT61.c
 *
 *  Created on: May 27, 2024
 *      Author: LUCKBACK
 */
//#include "i2c.h"
#include "WT61.h"

struct SAcc 	stcAcc={0};
struct SGyro 	stcGyro={0};
struct SAngle 	stcAngle={0};
uint8_t GYRO_data;

float gyroZ_low_pass = 0.0f; // 陀螺仪Z轴低通滤波值后的值

void WT61_WriteCommand(uint8_t Command)
{
	uint8_t key[] = {WRITE_HEAD_H,WRITE_HEAD_L,Command};
	HAL_UART_Transmit(WT61_SERIAL_HANDLE,key,3,100);
}

void WT61_ChangeBaudrate(uint32_t baudrate)
{
//  HAL_UART_DeInit(GYRO_USART_HANDLE);
//  (*GYRO_USART_HANDLE).Init.BaudRate = baudrate;
//  HAL_UART_Init(GYRO_USART_HANDLE);
}

void WT61_ReadData(void)
{
	HAL_UART_Receive_IT(WT61_SERIAL_HANDLE, &GYRO_data, 1);
}

/**
 * @brief Z轴角度归零
 */
void WT61_ClearZAngle(void)
{
	WT61_WriteCommand(CLEAR_Z_ANGLE);
}

/**
 * @brief 加计校准
 */
void WT61_AccCalibration(void)
{
	WT61_WriteCommand(ACC_CALIBRATION);
}

/**
 * @brief 休眠与解休眠,
 * 		  若当前为工作状态，执行指令进入休眠状态
 * 		  若当前为休眠状态，执行指令进入工作状态
 */
void WT61_Hibernation(void)
{
	WT61_WriteCommand(HIBERNATION);
}

/**
 * @brief 切换模式
 * @param mode 0为串口模式，1为i2c模式
 */
void WT61_ModeType(uint8_t mode)
{
	if(mode == 0)
		WT61_WriteCommand(SERIAL_MODE);
	else if (mode == 1)
		WT61_WriteCommand(I2C_MODE);
}

/**
 * @brief 设置串口速率
 * @param baudrate 设置波特率 115200 或9600
 */
void WT61_SetBaudrate(uint32_t baudrate)
{
	if(baudrate == 115200)
	{
		WT61_ChangeBaudrate(9600);
		WT61_WriteCommand(BAUDRATE_115200);
		WT61_ChangeBaudrate(115200);
	}
	else if(baudrate == 9600)
	{
		WT61_ChangeBaudrate(115200);
		WT61_WriteCommand(BAUDRATE_9600);
		WT61_ChangeBaudrate(9600);
	}
}

/**
 * @brief 设置安装位置
 * @param position 设置位置 0 为水平位置 1为垂直位置
 */
void WT61_Install(uint8_t position)
{
	if(position == 0)
		WT61_WriteCommand(HORIZONTAL_INSTALLATION);
	else if(position == 1)
		WT61_WriteCommand(VERTICAL_INSTALLATION);
}

void WT61_Init(void)
{
	WT61_ClearZAngle();
	WT61_AccCalibration();
	WT61_ReadData();
}

/**
 * @brief 获取数据
 */
void CopeSerialData(unsigned char ucData)
{
	static unsigned char ucRxBuffer[12];
	static unsigned char ucRxCnt = 0;
	static unsigned char sumcrc = 0;

	ucRxBuffer[ucRxCnt++]=ucData;
	if (ucRxBuffer[0]!=0x55) //数据头不对，则重新开始寻找0x55数据头
	{
		ucRxCnt=0;
		sumcrc = 0;
		return ;
	}
	if (ucRxCnt<11) {sumcrc += ucData; return ;}//数据不满11个，则返回
	else
	{
		if (sumcrc != ucRxBuffer[10])
		{
			ucRxCnt=0;
			sumcrc = 0;
			return ;
		}
		switch(ucRxBuffer[1])
		{
			case READ_TYPE_ACC:		stcAcc.a[0] = ((float)(short)((short)ucRxBuffer[3]<<8|ucRxBuffer[2])/32768)*16*9.8;
									stcAcc.a[1] = ((float)(short)((short)ucRxBuffer[5]<<8|ucRxBuffer[4])/32768)*16*9.8;
									stcAcc.a[2] = ((float)(short)((short)ucRxBuffer[7]<<8|ucRxBuffer[6])/32768)*16*9.8;
									break;
			case READ_TYPE_GYRO:	stcGyro.w[0] = ((float)(short)((short)ucRxBuffer[3]<<8|ucRxBuffer[2])/32768)*2000;
									stcGyro.w[1] = ((float)(short)((short)ucRxBuffer[5]<<8|ucRxBuffer[4])/32768)*2000;
									stcGyro.w[2] = ((float)(short)((short)ucRxBuffer[7]<<8|ucRxBuffer[6])/32768)*2000;
									gyroZ_low_pass = 0.5f * gyroZ_low_pass + 0.5f * stcGyro.w[2]; // 陀螺仪Z轴低通滤波
									break;
			case READ_TYPE_ANGLE:	stcAngle.Angle[0] = ((float)(short)((short)ucRxBuffer[3]<<8|ucRxBuffer[2])/32768)*180;
									stcAngle.Angle[1] = ((float)(short)((short)ucRxBuffer[5]<<8|ucRxBuffer[4])/32768)*180;
									stcAngle.Angle[2] = ((float)(short)((short)ucRxBuffer[7]<<8|ucRxBuffer[6])/32768)*180;
									stcAngle.Version = ((unsigned short)ucRxBuffer[9]<<8)|ucRxBuffer[8];
									break;
		}
		ucRxCnt=0;
		sumcrc = 0;
	}
	return ;
}

//void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
//{
//	CopeSerialData(data);
//	WT61_ReadData();
//}
float getGyroZ(){
	return gyroZ_low_pass; // 返回低通滤波后的陀螺仪Z轴数据
}
